/**************************************************************
 * 
 * @brief

 ***************************************************************/
#include "ALL_DEFINE.h"

volatile uint32_t SysTick_count; //系统时间计数
_st_Mpu MPU6050;                 //MPU6050原始数据
_st_Mag AK8975;
//_st_AngE Angle;    //当前角度姿态值
_st_Remote Remote; //遥控通道值

_st_ALL_flag ALL_flag; //系统标志位，包含解锁标志位等

PidObject pidRateX; //内环PID数据
PidObject pidRateY;
PidObject pidRateZ;

PidObject pidPitch; //外环PID数据
PidObject pidRoll;
PidObject pidYaw;

PidObject pidHeightRate;
PidObject pidHeightHigh;

void pid_param_Init(void); //PID控制参数初始化，改写PID并不会保存数据，请调试完成后直接在程序里更改 再烧录到飞控

/**************************************************************
 *  整个系统外设和传感器初始化
 * @param[in] 
 * @param[out] 
 * @return     
 ***************************************************************/
void ALL_Init(void)
{
    //  IIC_Init();             //I2C初始化

    //  pid_param_Init();       //PID参数初始化
    //
    //  LEDInit();              //LED闪灯初始化
    //
    //  delay_ms(5000);         //延时5S等待气压计稳定
    //
    //  MpuInit();              //MPU6050初始化
    //
    //  USART3_Config();        //上位机串口初始化

    //  NRF24L01_init();        //2.4G遥控通信初始化
    //
    //  TIM2_PWM_Config();      //4路PWM初始化
    //
    //
    //  TIM3_Config();          //系统工作周期初始化
}

/**************************************************************
 *  初始化PID参数
 * @brief 如果需要自己修改PID值，调这里就可以了
 * @param[out] 
 * @return     
 ***************************************************************/
void pid_param_Init(void)
{
    pidRateX.kp = 4.0f;
    pidRateY.kp = 4.0f;
    pidRateZ.kp = 8.0f;

    pidRateX.ki = 0.0f;
    pidRateY.ki = 0.0f;
    pidRateZ.ki = 0.0f;

    pidRateX.kd = 0.28f;
    pidRateY.kd = 0.28f;
    pidRateZ.kd = 0.4f;

    pidPitch.kp = 6.0f;
    pidRoll.kp = 6.0f;
    pidYaw.kp = 6.0f;

    pidHeightRate.kp = 1.3f; //0.5f
    pidHeightRate.kd = 0.35f;

    //  pidHeightHigh.ki = 0.05f;
    pidHeightHigh.kp = 0.3f; //0.32f
}
